#include "solver_types.h"
#include "GMRES.h"
//#include "record.h"

template<typename data_t>
static void block_trsv(int nrows, const data_t* M, data_t* x) {
	// L*U*x = b
	if constexpr (sizeof(data_t) == 4) {
		cblas_strsv(CblasColMajor, CblasLower, CblasNoTrans, CblasUnit	 , nrows, M, nrows, x, 1);// U*x =        L^{-1}*b
		cblas_strsv(CblasColMajor, CblasUpper, CblasNoTrans, CblasNonUnit, nrows, M, nrows, x, 1);//   x = U^{-1}*L^{-1}*b
	}
	else if constexpr (sizeof(data_t) == 8) {
		cblas_dtrsv(CblasColMajor, CblasLower, CblasNoTrans, CblasUnit	 , nrows, M, nrows, x, 1);// U*x =        L^{-1}*b
		cblas_dtrsv(CblasColMajor, CblasUpper, CblasNoTrans, CblasNonUnit, nrows, M, nrows, x, 1);//   x = U^{-1}*L^{-1}*b
	}
}

template<typename data_t, typename calc_t>
static void apply_LU(int nrows, const data_t* M, const calc_t* b, calc_t* x, data_t* buf) {
	data_t* sol = nullptr;
	// 因为trsv是原位写回的，所以先将右端项拷贝到要求解的向量里
	if constexpr (sizeof(data_t) != sizeof(calc_t)) {
		copy_vec(nrows, b, buf);
		sol = buf;
	}
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(nrows, b, 1, x, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(nrows, b, 1, x, 1);
		sol = x;
	}

	block_trsv(nrows, M, sol);

	// 锟斤拷罂交氐锟斤拷锟斤拷锟斤拷
	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(nrows, sol, x);
}

template<typename data_t, typename calc_t>
static void apply_block_LU(int nrows, const data_t* M_both, const calc_t* b, calc_t* x, data_t* buf) {
	int hf = nrows >> 1;
	data_t* sol = nullptr;
	// 因为trsv是原位写回的，所以先将右端项拷贝到要求解的向量里
	if constexpr (sizeof(data_t) != sizeof(calc_t)) {
		copy_vec(nrows, b, buf);
		sol = buf;
	}
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(nrows, b, 1, x, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(nrows, b, 1, x, 1);
		sol = x;
	}
	
	block_trsv(hf, M_both		 , sol     );
	block_trsv(hf, M_both + hf*hf, sol + hf);

	// 最后拷回到结果里
	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(nrows, sol, x);
}

static int* ipiv = nullptr;
template<typename data_t, typename calc_t>
static void apply_block_LU_pivot(int nrows, const data_t* M_both, const calc_t* b, calc_t* x, data_t* buf) {
	int hf = nrows >> 1;
	data_t* sol = nullptr;
	// 因为getrs是原位写回的，所以先将右端项拷贝到要求解的向量里
	if constexpr (sizeof(data_t) != sizeof(calc_t)) {
		copy_vec(nrows, b, buf);
		sol = buf;
	}
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(nrows, b, 1, x, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(nrows, b, 1, x, 1);
		sol = x;
	}
	int info;// mkl 返回值
	if constexpr (sizeof(data_t) == 4) {
		info = LAPACKE_sgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both		  , hf, ipiv	 , sol	   , hf); assert(info == 0);// overwrite
		info = LAPACKE_sgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both + hf*hf, hf, ipiv + hf, sol + hf, hf); assert(info == 0);
	} else if constexpr (sizeof(data_t) == 8) {
		info = LAPACKE_dgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both		  , hf, ipiv	 , sol	   , hf); assert(info == 0);// overwrite
		info = LAPACKE_dgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both + hf*hf, hf, ipiv + hf, sol + hf, hf); assert(info == 0);
	}
	// 最后拷回到结果里
	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(nrows, sol, x);
}

static void* A10 = nullptr;
template<typename data_t, typename calc_t>
static void apply_block_LU_1C(int nrows, const data_t* M_both, const calc_t* b, calc_t* x, data_t* buf) {
	assert(A10);
	int hf = nrows >> 1;
	data_t* sol = nullptr;
	// 因为trsv是原位写回的，所以先将右端项拷贝到结果里
	if constexpr (sizeof(data_t) != sizeof(calc_t)) {
		copy_vec(nrows, b, buf);
		sol = buf;
	}
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(nrows, b, 1, x, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(nrows, b, 1, x, 1);
		sol = x;
	}
	
	block_trsv(hf, M_both, sol);
	if 		  constexpr (sizeof(data_t) == 4) {
		cblas_sgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (float *)A10, nrows, sol, 1, 1.0, sol + hf, 1);
	} else if constexpr (sizeof(data_t) == 8) {
		cblas_dgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (double*)A10, nrows, sol, 1, 1.0, sol + hf, 1);
	}
	block_trsv(hf, M_both + hf*hf, sol + hf);

	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(nrows, sol, x);
}

template<typename data_t, typename calc_t>
static void apply_block_LU_1C_pivot(int nrows, const data_t* M_both, const calc_t* b, calc_t* x, data_t* buf) {
	assert(A10);
	int hf = nrows >> 1;
	data_t* sol = nullptr;
	// 因为trsv是原位写回的，所以先将右端项拷贝到结果里
	if constexpr (sizeof(data_t) != sizeof(calc_t)) {
		copy_vec(nrows, b, buf);
		sol = buf;
	}
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(nrows, b, 1, x, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(nrows, b, 1, x, 1);
		sol = x;
	}
	int info;// mkl 返回值
	if constexpr (sizeof(data_t) == 4) {
		info = LAPACKE_sgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both		  , hf, ipiv	 , sol, hf	   ); assert(info == 0);// 相当于 block_trsv(hf, M_both, sol);
		cblas_sgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (float *)A10, nrows, sol, 1, 1.0, sol + hf, 1);
		info = LAPACKE_sgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both + hf*hf, hf, ipiv + hf, sol + hf, hf); assert(info == 0);// block_trsv(hf, M_both + hf*hf, sol + hf);
	} else if constexpr (sizeof(data_t) == 8) {
		info = LAPACKE_dgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both		  , hf, ipiv	  , sol, hf	    ); assert(info == 0);
		cblas_dgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (double*)A10, nrows, sol, 1, 1.0, sol + hf, 1);
		info = LAPACKE_dgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both + hf*hf, hf, ipiv + hf, sol + hf, hf); assert(info == 0);// block_trsv(hf, M_both + hf*hf, sol + hf);
	}
	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(nrows, sol, x);
}

static void* A01 = nullptr;
template<typename data_t, typename calc_t>
static void apply_block_LU_2C(int nrows, const data_t* M_both, const calc_t* b, calc_t* x, data_t* buf) {
	static_assert(sizeof(data_t) == 4);
	assert(A10);
	assert(A01);
	int hf = nrows >> 1;
	data_t* sol;
	// 因为trsv是原位写回的，所以先将右端项拷贝到结果里
	if constexpr (sizeof(data_t) != sizeof(calc_t)) {
		copy_vec(nrows, b, buf);
		sol = buf;
	}
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(nrows, b, 1, x, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(nrows, b, 1, x, 1);
		sol = x;
	}

	block_trsv(hf, M_both, sol);
	if 		  constexpr (sizeof(data_t) == 4) {
		cblas_sgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (float *)A10, nrows, sol, 1, 1.0, sol + hf, 1);
	} else if constexpr (sizeof(data_t) == 8) {
		cblas_dgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (double*)A10, nrows, sol, 1, 1.0, sol + hf, 1);
	}
	block_trsv(hf, M_both + hf*hf, sol + hf);
	// 要先将b0拷到x0里，再算b0-A01*x1
	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(hf, b, sol);
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(hf, b, 1, sol, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(hf, b, 1, sol, 1);
		
	}
	if 		  constexpr (sizeof(data_t) == 4) {
		cblas_sgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (float *)A01, nrows, sol + hf, 1, 1.0, sol, 1);
	} else if constexpr (sizeof(data_t) == 8) {
		cblas_dgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (double*)A01, nrows, sol + hf, 1, 1.0, sol, 1);
	}
	block_trsv(hf, M_both, sol);

	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(nrows, sol, x);
}

template<typename data_t, typename calc_t>
static void apply_block_LU_2C_pivot(int nrows, const data_t* M_both, const calc_t* b, calc_t* x, data_t* buf) {
	static_assert(sizeof(data_t) == 4);
	assert(A10);
	assert(A01);
	int hf = nrows >> 1;
	data_t* sol;
	// 因为trsv是原位写回的，所以先将右端项拷贝到结果里
	if constexpr (sizeof(data_t) != sizeof(calc_t)) {
		copy_vec(nrows, b, buf);
		sol = buf;
	}
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(nrows, b, 1, x, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(nrows, b, 1, x, 1);
		sol = x;
	}


	int info;// mkl 返回值
	if 		  constexpr (sizeof(data_t) == 4) {
		info = LAPACKE_sgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both		  , hf, ipiv	 , sol, hf	   ); assert(info == 0);// 相当于 block_trsv(hf, M_both, sol);
		cblas_sgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (float *)A10, nrows, sol, 1, 1.0, sol + hf, 1);
		info = LAPACKE_sgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both + hf*hf, hf, ipiv + hf, sol + hf, hf); assert(info == 0);// block_trsv(hf, M_both + hf*hf, sol + hf);
	} else if constexpr (sizeof(data_t) == 8) {
		info = LAPACKE_dgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both		  , hf, ipiv	 , sol, hf	   ); assert(info == 0);
		cblas_dgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (double*)A10, nrows, sol, 1, 1.0, sol + hf, 1);
		info = LAPACKE_dgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both + hf*hf, hf, ipiv + hf, sol + hf, hf); assert(info == 0);
	}
	// 要先将b0拷到x0里，再算b0-A01*x1
	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(hf, b, sol);
	else {
		if 		constexpr (sizeof(calc_t) == 4) cblas_scopy(hf, b, 1, sol, 1);
		else if constexpr (sizeof(calc_t) == 8) cblas_dcopy(hf, b, 1, sol, 1);
		
	}
	if 		  constexpr (sizeof(data_t) == 4) {
		cblas_sgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (float *)A01, nrows, sol + hf, 1, 1.0, sol, 1);
		info = LAPACKE_sgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both, hf, ipiv, sol, hf); assert(info == 0);// 相当于 block_trsv(hf, M_both, sol);
	} else if constexpr (sizeof(data_t) == 8) {
		cblas_dgemv(CblasColMajor, CblasNoTrans, hf, hf, -1.0, (double*)A01, nrows, sol + hf, 1, 1.0, sol, 1);
		info = LAPACKE_dgetrs(LAPACK_COL_MAJOR, 'N', hf, 1, M_both, hf, ipiv, sol, hf);
	}
	if constexpr (sizeof(data_t) != sizeof(calc_t))
		copy_vec(nrows, sol, x);
}

static inline void invoke_iter(int nrows, 
	const double* A_64, const double* rhs_64, double* sol_64, void (*pc_ptr_64)(int, const float*, const double*, double*, float*),
	const float* A_32, const float* rhs_32, float* sol_32, void (*pc_ptr_32)(int, const float*, const float*, float*, float*),
	float* pc_mem, double* delta_b, double* tmp_sol, int max_iter, int restart_len,
	double atol_32, double atol_64, double atol_rf,
	double rtol_32, double rtol_64, double rtol_rf, int & iter_32, int & iter_64, int & iter_rf)
{
	// 阶段一
	zero_vec(nrows, sol_32);// 清零结果
	iter_32 = GMRES<float, float>(nrows, A_32, rhs_32, sol_32, atol_32, rtol_32, max_iter, restart_len, true, pc_ptr_32, pc_mem);
	// 阶段二
	copy_vec<float, double>(nrows, sol_32, sol_64);
	iter_64 = GMRES<float, double>(nrows, A_64, rhs_64, sol_64, atol_64, rtol_64, max_iter, restart_len, false, pc_ptr_64, pc_mem);
	// 阶段三
	cblas_dcopy(nrows, rhs_64, 1, delta_b, 1);
	cblas_dcopy(nrows, sol_64, 1, tmp_sol, 1);// 暂存中间结果
	cblas_dgemv(CblasColMajor, CblasNoTrans, nrows, nrows, -1.0, A_64, nrows, sol_64, 1, 1.0, delta_b, 1);// 锟斤拷b := b - A*x
	zero_vec(nrows, sol_64);
	iter_rf = GMRES<float, double>(nrows, A_64, delta_b, sol_64, atol_rf, rtol_rf, max_iter, restart_len, true, pc_ptr_64, pc_mem);
	cblas_daxpy(nrows, 1.0, tmp_sol, 1, sol_64, 1);// 将中间结果累加回来
}

float*  iterat_solver(const int nrows, const double* A_64, double* rhs_64, int pc_type, TEST_TIME & record,
			const int last_nrows, const float * last_mem)
{
	auto beg = std::chrono::high_resolution_clock::now();
	auto end_setup = beg;

	int iter_32 = 0, iter_64 = 0, iter_rf = 0;
	float* A_32 = new float[(long long)nrows * nrows];
	float* rhs_32 = new float[nrows], * sol_32 = new float[nrows];
	double* sol_64 = new double[nrows];
	double* delta_b = new double[nrows], * tmp_sol = new double [nrows];
	// auto alloc_end = std::chrono::high_resolution_clock::now();
	// printf("Alloc %.6f s\n", (std::chrono::duration_cast<std::chrono::nanoseconds>(alloc_end - beg)).count() * 1e-9);

	float* M = nullptr;
	copy_vec<double, float>(nrows, rhs_64, rhs_32);
	copy_vec<double, float>(nrows * nrows, A_64, A_32);

	const int max_iter = 100;
	// double atol_32 = 1e2, atol_64 = 1e-5, atol_rf = 5e-8;
	// double rtol_32 = 0.0, rtol_64 = 0.0 , rtol_rf = 0.0 ;
	double atol_32 = 0.0, atol_64 = 0.0, atol_rf = 0.0;
	double rtol_32 = 1e-4, rtol_64 = 1e-10, rtol_rf = 1e-2;
	int restart_len = 3;

	void (*pc_ptr_32)(int, const float*, const float*, float*, float*) = nullptr;
	void (*pc_ptr_64)(int, const float*, const double*, double*, float*) = nullptr;
	switch (pc_type)
	{
	case 0:
	case 91: 	pc_ptr_32 = apply_LU		 <float, float>; pc_ptr_64 = apply_LU         <float, double>; break;
	case 1:
	case 11: 	pc_ptr_32 = apply_block_LU   <float, float>; pc_ptr_64 = apply_block_LU   <float, double>; break;
	case 2:
	case 21: 	pc_ptr_32 = apply_block_LU_1C<float, float>; pc_ptr_64 = apply_block_LU_1C<float, double>; break;
	case 3:
	case 31: 	pc_ptr_32 = apply_block_LU_2C<float, float>; pc_ptr_64 = apply_block_LU_2C<float, double>; break;
	case 111: 	pc_ptr_32 = apply_block_LU_pivot<float, float>; pc_ptr_64 = apply_block_LU_pivot<float, double>; break;
	case 211: 	pc_ptr_32 = apply_block_LU_1C_pivot<float, float>; pc_ptr_64 = apply_block_LU_1C<float, double>; break;
	case 311: 	pc_ptr_32 = apply_block_LU_2C_pivot<float, float>; pc_ptr_64 = apply_block_LU_2C<float, double>; break;
	default: break;
	}

	if (pc_type == 0) {// 单精度直接法
		int info;// mkl_lapack的返回值
		M = (float*) malloc(sizeof(*M) * nrows * nrows); assert(M);
		copy_vec(nrows * nrows, A_32, M);
#ifdef _OPEN
		int * ipiv = new int [nrows];
		LAPACK_sgetrf(&nrows, &nrows, M, &nrows, ipiv, &info);
		for (int i = 0; i < nrows; i++) assert(ipiv[i] == i+1);
		delete[] ipiv;
#else
		info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, nrows, nrows, M, nrows); assert(info == 0);// 不带pivot的LU分解
#endif
		end_setup = std::chrono::high_resolution_clock::now();
		invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
			atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		free(M); M = nullptr;
	}
	else if (pc_type == 91) {
		int info;// mkl_lapack的返回值
		int hf = nrows >> 1;
		M = (float*) malloc(sizeof(*M) * nrows * nrows); assert(M);
		if (last_mem == nullptr) {
			copy_vec(nrows * nrows, A_32, M);
#ifdef _OPEN
			int * ipiv = new int [nrows];
			LAPACK_sgetrf(&nrows, &nrows, M, &nrows, ipiv, &info);
			for (int i = 0; i < nrows; i++) assert(ipiv[i] == i+1);
			delete[] ipiv;
#else
			info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, nrows, nrows, M, nrows); assert(info == 0);// 不带pivot的LU分解
#endif
			end_setup = std::chrono::high_resolution_clock::now();
			invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
				atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		}
		else {
			copy_vec(nrows * nrows, A_32, M);
			extend_full(last_mem, last_nrows, M, nrows);
			end_setup = std::chrono::high_resolution_clock::now();
			invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
				atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		}
	}
	else if (pc_type == 1  || pc_type == 2  || pc_type == 3 ) {
		int info;// mkl_lapack锟侥凤拷锟斤拷值
		int hf = nrows >> 1;
		M = (float*) malloc(sizeof(*M) * hf * hf * 2); assert(M);
		float* M_0 = M, * M_1 = M + hf * hf;
		A10 = A_32 + hf;
		A01 = A_32 + hf * nrows;
		copy_mat(A_32				   , nrows, M_0, hf, hf, hf);
		copy_mat(A_32 + hf * nrows + hf, nrows, M_1, hf, hf, hf);
#ifdef _OPEN
		int * ipiv = new int [hf];
		LAPACK_sgetrf(&hf, &hf, M		 , &hf, ipiv, &info); assert(info == 0);
		for (int i = 0; i < hf; i++) assert(ipiv[i] == i+1);
		LAPACK_sgetrf(&hf, &hf, M + hf*hf, &hf, ipiv, &info); assert(info == 0);
		for (int i = 0; i < hf; i++) assert(ipiv[i] == i+1);
		delete[] ipiv;
#else
		info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, hf, hf, M          , hf); assert(info == 0);
		info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, hf, hf, M + hf * hf, hf); assert(info == 0);
#endif
		end_setup = std::chrono::high_resolution_clock::now();
		invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
			atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		A01 = nullptr;
		A10 = nullptr;
		free(M); M = nullptr;
	}
	else if (pc_type == 11 || pc_type == 21 || pc_type == 31) {
		int hf = nrows >> 1;
		M = (float*) malloc(sizeof(*M) * hf * hf * 2); assert(M);
		float* M_0 = M, * M_1 = M + hf * hf;
		A10 = A_32 + hf;
		A01 = A_32 + hf * nrows;
		copy_mat(A_32				   , nrows, M_0, hf, hf, hf);
		copy_mat(A_32 + hf * nrows + hf, nrows, M_1, hf, hf, hf);
		if (last_mem == nullptr) {
			int info;
#ifdef _OPEN
			int * ipiv = new int [hf];
			LAPACK_sgetrf(&hf, &hf, M		 , &hf, ipiv, &info); assert(info == 0);
			for (int i = 0; i < hf; i++) assert(ipiv[i] == i+1);
			LAPACK_sgetrf(&hf, &hf, M + hf*hf, &hf, ipiv, &info); assert(info == 0);
			for (int i = 0; i < hf; i++) assert(ipiv[i] == i+1);
			delete[] ipiv;
#else
			info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, hf, hf, M			 , hf); assert(info == 0);
			info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, hf, hf, M + hf * hf, hf); assert(info == 0);
#endif
			end_setup = std::chrono::high_resolution_clock::now();
			invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
				atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		}
		else {
			int last_hf = last_nrows >> 1;
			extend_block(last_mem					 , last_hf, M_0, hf);
			extend_block(last_mem + last_hf * last_hf, last_hf, M_1, hf);
			end_setup = std::chrono::high_resolution_clock::now();
			invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
				atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		}
		A01 = nullptr;
		A10 = nullptr;
	}
	else if (pc_type == 111 || pc_type == 211 || pc_type == 311) {
		int hf = nrows >> 1;
		M = (float*) malloc(sizeof(*M) * hf * hf * 2 + sizeof(int) * nrows); assert(M);// 直接将ipiv的内存附在最后
		float* M_0 = M, * M_1 = M + hf * hf;
		int* ip_0 = (int*) (((uint64_t)M) + sizeof(*M) * hf * hf * 2);
		int* ip_1 = ip_0 + hf;
		copy_mat(A_32				   , nrows, M_0, hf, hf, hf);
		copy_mat(A_32 + hf * nrows + hf, nrows, M_1, hf, hf, hf);
		A10 = A_32 + hf;
		A01 = A_32 + hf * nrows;
		ipiv = ip_0;
		if (last_mem == nullptr) {
			int info;
			info = LAPACKE_sgetrf(LAPACK_COL_MAJOR, hf, hf, M_0, hf, ip_0); assert(info == 0);
			info = LAPACKE_sgetrf(LAPACK_COL_MAJOR, hf, hf, M_1, hf, ip_1); assert(info == 0);
			end_setup = std::chrono::high_resolution_clock::now();
			invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
				atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		}
		else {
			int last_hf = last_nrows >> 1;
			const int* last_ip_0 = (const int*) (((uint64_t)last_mem) + sizeof(*last_mem) * last_hf * last_hf * 2);
			const int* last_ip_1 = last_ip_0 + last_hf;
			for (int i = 0; i < last_hf; i++) {// 继承上一时步的重排关系
				ip_0[i] = last_ip_0[i];
				ip_1[i] = last_ip_1[i];
			}
			extend_block(last_mem					 , last_hf, M_0, hf, ip_0);
			extend_block(last_mem + last_hf * last_hf, last_hf, M_1, hf, ip_1);
			end_setup = std::chrono::high_resolution_clock::now();
			invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
				atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		}
		A01 = nullptr;
		A10 = nullptr;
		ipiv = nullptr;
	}
	else {// 无预条件
		invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, nullptr, delta_b, tmp_sol, max_iter, restart_len,
			atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);

		auto end = std::chrono::high_resolution_clock::now();
		auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg);
		record.t_solve += ((double)elapsed.count() * 1e-9);
	}
	
	copy_vec(nrows, sol_64, rhs_64);
	delete[] A_32; delete[] rhs_32; delete[] sol_32;
	delete[] delta_b; delete[] tmp_sol; delete[] sol_64;

	auto end_solve = std::chrono::high_resolution_clock::now();
	record.t_setup = (std::chrono::duration_cast<std::chrono::nanoseconds>(end_setup - beg      )).count() * 1e-9;
	record.t_solve = (std::chrono::duration_cast<std::chrono::nanoseconds>(end_solve - end_setup)).count() * 1e-9;
	record.iter = abs(iter_32) + abs(iter_64) + abs(iter_rf);
	return M;
}

float*  iterat_reorder_solver(const int nrows, const double* A_64, double* rhs_64, int pc_type, TEST_TIME & record,
			const int last_nrows, const float * last_mem, const std::vector<int> & prev_hfs)
{
	auto beg = std::chrono::high_resolution_clock::now();
	auto end_setup = beg;

	int iter_32 = 0, iter_64 = 0, iter_rf = 0;
	float* A_32 = new float[(long long)nrows * nrows];
	float* rhs_32 = new float[nrows], * sol_32 = new float[nrows];
	double* sol_64 = new double[nrows];
	double* delta_b = new double[nrows], * tmp_sol = new double [nrows];

	float* M = nullptr;
	assert(pc_type == 91);
	void (*pc_ptr_32)(int, const float*, const float*, float*, float*) = nullptr;
	void (*pc_ptr_64)(int, const float*, const double*, double*, float*) = nullptr;
	pc_ptr_32 = apply_LU		 <float, float>; pc_ptr_64 = apply_LU         <float, double>;
	M = (float*) malloc(sizeof(*M) * nrows * nrows); assert(M);

	const int max_iter = 100;
	double atol_32 = 0.0, atol_64 = 0.0, atol_rf = 0.0;
	double rtol_32 = 1e-4, rtol_64 = 1e-10, rtol_rf = 1e-2;
	int restart_len = 3;

	if (last_mem == nullptr) {
		#pragma omp parallel for schedule(static) num_threads(gnt)
		for (int j = 0; j < nrows; j++) {
			for (int i = 0; i < nrows; i++) {
				const int pos = j * nrows + i;
				M   [pos] = A_64[pos];
				A_32[pos] = A_64[pos];
			}
			rhs_32[j] = rhs_64[j];
		}
		int info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, nrows, nrows, M, nrows); assert(info == 0);// 不带pivot的LU分解
		end_setup = std::chrono::high_resolution_clock::now();
		invoke_iter(nrows, A_64, rhs_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
			atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
	}
	else {
		double* A_reorder_64 = new double [nrows * nrows];
		double* rhs_reorder_64 = new double [nrows];
		int* map_o2n = new int [nrows], * map_n2o = new int [nrows];
		int hf = nrows >> 1;
		int last_hf = last_nrows >> 1;
		// auto reorder_beg = std::chrono::high_resolution_clock::now();
		#pragma omp parallel num_threads(gnt)
		{
			#pragma omp for schedule(static)
			for (int g = 0; g < prev_hfs.size() - 1; g++) {
				int ibeg = prev_hfs[g],
					iend = prev_hfs[g + 1];
				// printf("[%d, %d)\n", ibeg, iend);
				for (int i = ibeg; i < iend; i++) {
					map_o2n[i] = (ibeg << 1) + i - ibeg;
					map_o2n[i + hf] = i + iend;
				}
			}
			#pragma omp for schedule(static)
			for (int i = 0; i < nrows; i++)
				map_n2o[map_o2n[i]] = i;
			#pragma omp for schedule(static)
			for (int nj = 0; nj < nrows; nj++) {
				const int oj = map_n2o[nj];
				for (int ni = 0; ni < nrows; ni++) {
					const int oi = map_n2o[ni];
					const double src_64 = A_64[oj * nrows + oi];
					const float  src_32 = (float) src_64;
					const int new_pos = nj * nrows + ni;
					A_reorder_64[new_pos] = src_64;
					A_32		[new_pos] = src_32;
					M			[new_pos] = src_32;
				}
				rhs_reorder_64[nj] = rhs_64[oj];
				rhs_32		  [nj] = rhs_64[oj];
			}
		}
		// auto reorder_end = std::chrono::high_resolution_clock::now();
		// printf("reorder %.6f s\n", (std::chrono::duration_cast<std::chrono::nanoseconds>(reorder_end - reorder_beg)).count() * 1e-9);
		
		extend_block(last_mem, last_nrows, M, nrows);
		end_setup = std::chrono::high_resolution_clock::now();
		invoke_iter(nrows, A_reorder_64, rhs_reorder_64, sol_64, pc_ptr_64, A_32, rhs_32, sol_32, pc_ptr_32, M, delta_b, tmp_sol, max_iter, restart_len,
			atol_32, atol_64, atol_rf, rtol_32, rtol_64, rtol_rf, iter_32, iter_64, iter_rf);
		// 拷回
		#pragma omp parallel for schedule(static)
		for (int i = 0; i < nrows; i++) {
			rhs_64[i] = sol_64[map_o2n[i]];
		}
		delete[] A_reorder_64; delete[] rhs_reorder_64; delete[] map_o2n; delete[] map_n2o;
	}
	delete[] A_32; delete[] rhs_32; delete[] sol_32;
	delete[] delta_b; delete[] tmp_sol; delete[] sol_64;

	auto end_solve = std::chrono::high_resolution_clock::now();
	record.t_setup = (std::chrono::duration_cast<std::chrono::nanoseconds>(end_setup - beg      )).count() * 1e-9;
	record.t_solve = (std::chrono::duration_cast<std::chrono::nanoseconds>(end_solve - end_setup)).count() * 1e-9;
	record.iter = abs(iter_32) + abs(iter_64) + abs(iter_rf);
	return M;
}